#include <general_interface/action/auto_task.hpp>
#include <general_interface/action/switch_connection.hpp>
#include <rclcpp/rclcpp.hpp>
#include <rclcpp_action/rclcpp_action.hpp>

class DisconnectTask : public rclcpp::Node
{
  private:
    rclcpp_action::Server<general_interface::action::AutoTask>::SharedPtr action_server;
    rclcpp_action::Client<general_interface::action::SwitchConnection>::SharedPtr switch_client;

  public:
    explicit DisconnectTask(const std::string &node_name) : Node(node_name)
    {
        action_server = rclcpp_action::create_server<general_interface::action::AutoTask>(
            this, "disconnect_task",
            std::bind(&DisconnectTask::on_goal, this, std::placeholders::_1, std::placeholders::_2),
            std::bind(&DisconnectTask::on_cancel, this, std::placeholders::_1),
            std::bind(&DisconnectTask::on_accept, this, std::placeholders::_1));
        switch_client = rclcpp_action::create_client<general_interface::action::SwitchConnection>(this, "connection");
    }

  private:
    rclcpp_action::GoalResponse on_goal(const rclcpp_action::GoalUUID &uuid,
                                        std::shared_ptr<const general_interface::action::AutoTask::Goal> goal)
    {
        (void)uuid;
        if (goal->task_id != general_interface::action::AutoTask::Goal::DISCONNECT)
        {
            RCLCPP_ERROR(get_logger(), "Invalid task_id received");
            return rclcpp_action::GoalResponse::REJECT;
        }
        return rclcpp_action::GoalResponse::ACCEPT_AND_EXECUTE;
    }

    rclcpp_action::CancelResponse on_cancel(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<general_interface::action::AutoTask>> goalHandle)
    {
        (void)goalHandle;
        return rclcpp_action::CancelResponse::ACCEPT;
    }

    void on_accept(
        const std::shared_ptr<rclcpp_action::ServerGoalHandle<general_interface::action::AutoTask>> goalHandle)
    {
        (void)goalHandle;
    }
};

int main(int argc, char **argv)
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<DisconnectTask>("disconnect_task"));
    rclcpp::shutdown();
    return 0;
}